module FEM_module

export formnf!, numtog, gg, inv2, det2, fkdiag!, fsparv!, sparin!, spabac!
export spabac_gauss!, sparin_gauss!, gauss_band!, solve_band!, formtb!, bantmul!
export bandwidth, lagrange, extract, fem_dot

function inv2(j::Array{Float64,2})
   det = -j[1,2]*j[2,1] + j[1,1]*j[2,2]
   res = [j[2,2] -j[1,2]; -j[2,1] j[1,1]]/det
   return res
end

function det2(j::Array{Float64,2})
   res = -j[1,2]*j[2,1] + j[1,1]*j[2,2];
   return res
end

function formnf!(nf::Array{Int64,2})
# nf is one matrix, with nodof rows and nn columns,
# where the entry should be assigned as 0 if the nodal dof/dofs is are suppressed
# new nf will be returned as the nodal dof numbering of the whole elements, in an order of nodal numbering
(imax, jmax) = size(nf)
m = 0
for j = 1:jmax
    for i = 1:imax
        if nf[i, j] != 0
            m = m + 1; nf[i, j] = m
        end
    end
end
return nothing
end

function numtog(num::Array{Int64,1}, nf::Array{Int64,2})
    # num denotes the nodal numbering of the current element, which must be integers.
    # num is a vector
    # nf is the nodal dof numbering of the whole structure,in an order of nodal numbering
    # this functon will return the nodal dof numbering of the current element
    nod = size(num, 1)  # node num each element
    nodof = size(nf,1) # dof num each node
    g = zeros(Int64, nod*nodof)
    for i = 1:nod
        k = i*nodof
        g[k - nodof + 1 : k] = nf[:, num[i]]
    end
    return g
end

function gg(nf::Array{Int64,2}, ndof::Int64, gnum::Array{Int64,2})
# this function  used to form element-dof array
# nf: node-dof array
# ndof: dof each element
nels = size(gnum,1)
r = zeros(Int64, ndof, nels)
for iel = 1:nels
    num = gnum[iel,:]
    g = numtog(num, nf)
    r[:, iel] = g
end
return r
end

function fkdiag!(kdiag, g)
    # kdiag in and out
    idof = size(g,1)
    for i = 1:idof
        iwp1 = 1
        if g[i] != 0
            for j = 1:idof
                if g[j] != 0
                    im = g[i] - g[j] + 1
                    if im > iwp1
                        iwp1 = im
                    end
                end
            end
            k = g[i]
            if iwp1 > kdiag[k]
                kdiag[k] = iwp1
            end
        end
    end
    return nothing
end

function fsparv!(kv, km, g, kdiag)
    # kv in and out
    idof = size(g,1)
    for i = 1:idof
        k = g[i]
        if k != 0
            for j = 1:idof
                if g[j] != 0
                    iw = k - g[j]
                    if iw >= 0
                        ival = kdiag[k] - iw
                        kv[ival] = kv[ival] + km[i, j]
                    end
                end
            end
        end
    end
    return nothing
end

function sparin!(kv::Array{Float64,1}, kdiag::Array{Int64,1})
# This subroutine performs Cholesky factorisation on a symmetric
# skyline global matrix.
    n = size(kdiag, 1)
    kv[1] = sqrt(kv[1])
    local x
    @inbounds for i = 2:n
        ki = kdiag[i] - i
        l = kdiag[i-1] - ki + 1
        @inbounds for j = l:i
            x = kv[ki+j]
            kj = kdiag[j] - j
            if j != 1
                ll = kdiag[j-1] - kj + 1
                ll = max(l, ll)
                if ll != j
                    m = j - 1
                    @inbounds for k = ll:m
                        x = x - kv[ki + k]*kv[kj + k]
                    end
                end
            end
            kv[ki+j] = x/kv[kj+j]
        end
        kv[ki+i] = sqrt(x);
    end
    return nothing
end

function spabac!(kv, loads, kdiag)
    # loads in and out
    n = size(kdiag,1)
    loads[1] = loads[1]/kv[1]
    @inbounds for i = 2:n
        ki = kdiag[i] - i
        l = kdiag[i - 1] - ki + 1
        x = loads[i]
        if l != i
            m = i - 1
            @inbounds for j = 1:m
                x = x - kv[ki + j]*loads[j]
            end
        end
        loads[i] = x/kv[ki + i]
    end
    @inbounds for it = 2:n
        i = n + 2 - it
        ki = kdiag[i] - i
        x = loads[i]/kv[ki + i]
        loads[i] = x
        l = kdiag[i - 1] - ki + 1
        if l != i
            m = i - 1;
            @inbounds for k = l:m
                loads[k] = loads[k] - x*kv[ki + k]
            end
        end
    end
    loads[1] = loads[1]/kv[1]
    return nothing
end

function sparin_gauss!(kv,kdiag)
#
# This subroutine performs Gaussian factorisation of a skyline matrix.
#
# INTEGER,PARAMETER::iwp=SELECTED_REAL_KIND(15)
# INTEGER,INTENT(IN)::kdiag(:)
# REAL(iwp),INTENT(OUT)::kv(:)
# REAL(iwp)::num,den,fac,zero=0.0_iwp
# INTEGER::n,ii,i,j,k,l,kk,l1,l2,l3
    n = size(kdiag,1)
    @inbounds for j = 1 : (n-1)
        den = kv[ kdiag[j] ]
        ii = 0
        @inbounds for i = (j+1) : n
            ii = ii+1
            l = kdiag[i] - ii
            if (l-kdiag[i-1]) > 0.0
                num = kv[l]
                fac = num/den
                kk = -1
                @inbounds for k = i:n
                    kk = kk+1
                    l1 = kdiag[i+kk] - kk
                    l2 = l1 - ii
                    l3 = kdiag[i + kk - 1]
                    if (l2-l3) > 0.0
                        kv[l1] = kv[l1] - fac*kv[l2]
                    end
                end
            end
        end
    end
    return nothing
end

function spabac_gauss!(kv, loads, kdiag)
#
# This subroutine performs Gaussian forwrad and back-substitution on a
# skyline matrix.
#
# INTEGER,PARAMETER::iwp = SELECTED_REAL_KIND(15)
# REAL(iwp),INTENT(IN)::kv(:)
# REAL(iwp),INTENT(IN OUT)::loads(0:)
# INTEGER,INTENT(IN)::kdiag(:)
# REAL(iwp)::num,den,fac,asum,zero=0.0_iwp
# INTEGER::i,j,l,n,ii,jj,l1,l2
    n = size(kdiag, 1)
    @inbounds for j = 1:(n-1)
        den = kv[ kdiag[j] ]
        ii = 0
        @inbounds for i = (j+1) : n
            ii = ii + 1
            l = kdiag[i] - ii
            if l-kdiag[i-1] > 0.0
                num = kv[l]
                fac = num/den
                loads[i] = loads[i] - fac*loads[j]
            end
        end
    end
    loads[n] = loads[n]/kv[kdiag[n]]
    @inbounds for i = n-1:-1:1
        jj = 0
        asum = 0.0
        @inbounds for j = i+1 : n
            jj = jj + 1
            l1 = kdiag[i+jj] - jj
            l2 = kdiag[i + jj - 1]
            if (l1-l2) > 0.0
                asum = asum + kv[l1]*loads[j]
            end
        end
        loads[i] = (loads[i] - asum)/kv[kdiag[i]]
    end
    return nothing
end

function gauss_band!(pb::Array{Float64,2}, work::Array{Float64,2})
    # This subroutine performs gaussian reduction of an unsymmetric
    # banded matrix pb. Array work used as working space.
    # IMPLICIT NONE
    # INTEGER,PARAMETER::iwp=SELECTED_REAL_KIND(15)
    # REAL(iwp),INTENT(IN OUT)::pb(:,:),work(:,:)
    # REAL(iwp)::s,zero=0.0_iwp,small=1.e-10_iwp
    # INTEGER::n,iwp1,iq,iqp,iwp11,i,j,k,l,ip,k1
    zero = 0.0
    small = 1.e-10
    n = size(pb,1)
    iwp1 = ceil(Int64, (size(pb,2) - 1)/2 ) + 1
    iq = 2*iwp1-1
    iqp = iwp1
    iwp11 = iwp1 - 1
    for i = 1:iwp11
        for j=1:iq
            if j >= iwp1+i
                pb[i,j] = zero
                pb[n-i+1, j] = zero
            else
                pb[i,j] = pb[i, j+iwp1-i]
            end
        end
    end
    for k = 1:n
        l = k + iwp1 - 1
        if l>n
            l = n
        end
        ip = 0
        s = small
        for i = k:l
            if abs(pb[i,1]) <= s
                continue
            end
            s = abs(pb[i,1])
            ip = i
        end
    if ip == 0
        println("singular")
        break
    end
    if k == n
        break
    end
    work[iwp1, k] = ip
    iqp = iqp - 1
    j = iwp1 + ip - k
    if iqp<j
        iqp = j
    end
    if j != iwp1
        for j = 1:iqp
            s = pb[k,j]
            pb[k,j] = pb[ip,j]
            pb[ip,j] = s
        end
    end
    k1 = k + 1
    for i = k1:l
        s = pb[i,1]/pb[k,1]
        for j=2:iq
            if j>iqp
                pb[i,j-1] = pb[i,j]
            else
                pb[i, j-1] = pb[i,j] - s*pb[k,j]
            end
        end
        pb[i,iq] = zero
        work[i-k, k] = s
    end
    end
    return nothing
end

function solve_band!(pb, work, loads)
    # This subroutine performs Gaussian forward and back-substitution
    # on the reduced unsymmetric band matrix pb.
    # IMPLICIT NONE
    # INTEGER,PARAMETER::iwp=SELECTED_REAL_KIND(15)
    # REAL(iwp),INTENT(IN)::pb(:,:),work(:,:)
    # REAL(iwp),INTENT(OUT)::loads(0:)
    # INTEGER::iwp1,n,n1,i,iv,l,iq,iv1
    # REAL(iwp)::s,pt5=0.5_iwp
    pt5 = 0.5
    iwp1 = ceil(Int64, (size(pb,2)-1)/2) + 1
    n = size(pb, 1)
    iq = 2*iwp1 - 1
    n1 = n - 1
    for iv = 1:n1
        i = trunc(Int64, work(iwp1,iv) + pt5)
        if i != iv
            s = loads[iv]
            loads[iv] = loads[i]
            loads[i] = s
        end
        l = iv + iwp1 - 1
        if l>n
            l = n
        end
        iv1 = iv + 1
        for i = iv1:l
            loads[i] = loads[i] - work[i-iv, iv]*loads[iv]
        end
    end
    loads[n] = loads[n]/pb[n, 1]
    iv = n - 1
    while iv != 0
        s = loads[iv]
        l = iq
        if iv + l-1>n
            l = n-iv + 1
        end
        for i=2:l
            s = s - pb[iv,i]*loads[iv+i-1]
            loads[iv] = s/pb[iv, 1]
        end
        iv = iv - 1
    end
    return nothing
end

function formtb!(pb,km,g)
    # This subroutine assembles an unsymmetrical band matrix pb from
    # element constituent matrices km.
    # IMPLICIT NONE
    # INTEGER,PARAMETER::iwp=SELECTED_REAL_KIND(15)
    # REAL(iwp),INTENT(IN)::km(:,:)
    # INTEGER,INTENT(IN)::g(:)
    # REAL(iwp),INTENT(OUT)::pb(:,:)
    # INTEGER::i,j,idof,icd,iw
    idof = size(km,1)
    iw = ceil(Int64, (size(pb,2) - 1)/2 )
    for i = 1:idof
        if g(i) != 0
            for j = 1:idof
                if g(j) != 0
                    icd = g[j] - g[i] + iw + 1
                    pb[g[i], icd] = pb[g[i],icd] + km[i, j]
                end
            end
        end
    end
    return nothing
end

function bantmul!(kb, loads, ans)
    # This subroutine multiplies an unsymmetrical band kb by the vector loads.
    # Could be much improved for vector processors.
    # IMPLICIT NONE
    # INTEGER, PARAMETER :: iwp=SELECTED_REAL_KIND(15)
    # REAL(iwp), INTENT(IN)  :: kb(:,:), loads(0:)
    # REAL(iwp), INTENT(OUT) :: ans(0:)
    # INTEGER:: i, j, k, l, m, n, iw
    # REAL(iwp)::x, zero = 0.0_iwp
    zero = 0.0
    n = size(kb, 1)
    l = size(kb, 2)
    iw = ceil(Int64, (l-1)/2)
    for i = 1:n
        x = zero
        k = iw + 2
        for j = 1:l
            k = k - 1
            m = i - k + 1
            if m<=n && m>=1
                x = x + kb[i,j]*loads[m]
            end
        end
        ans[i] = x
    end
    return nothing
end


function extract(array, index)
    if index == 0
        res = 0.0
    else
        res = array[index]
    end
    return res
end

function maxval(a::Array{Int64,1})
    a1 = a[a[:] .> 0]
    if size(a1,1) != 0
        return maximum(a1)
    else
        println("empty")
        return nothing
    end
end

function minval(a::Array{Int64,1})
    a1 = a[a[:] .> 0]
    if size(a1,1) != 0
        return minimum(a1)
    else
        println("empty")
        return nothing
    end
end

function bandwidth(g)
    # This function finds the element bandwidth from g.
    # IMPLICIT NONE
    # INTEGER,INTENT(IN)::g(:)
    # INTEGER::nband
    nband = maxval(g) - minval(g)
    return nband
end

function lagrange(nodes::Vector{Float64}, ξ)
	len = length(nodes)
	r = zeros(len)
	for p=1:len
   		s = 1.0
		for i = 1:len
			if i != p
				s = s*(ξ - nodes[i])/(nodes[p] - nodes[i])
			end
		end
		r[p] = s
	end
	return r
end

function fem_dot(v1::Array{Float64}, v2::Array{Float64})
    n1 = length(v1)
    n2 = length(v2)
    if n1==n2
        s = 0.0
        for i=1:n1
            s = s + v1[i]*v2[i]
        end
    else
        println("The input arrays have different length")
        return nothing
    end
    return s
end

end
